#include "dji.h"
#include "can_common.h"
#include "motor_def.h"
#include "offline.h"
#include "powercontroller.h"
#include "rtcompiler.h"
#include "rtthread.h"
#include "rttypes.h"
#include "user_lib.h"
#include <stdint.h>


#define LOG_TAG              "dji"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>


static rt_slist_t dji_motor_list= {RT_NULL}; // 会在motor_task任务中遍历该指针数组进行pid计算

rt_inline void dji_motor_list_init(rt_slist_t *l)
{
    l->next = RT_NULL;//0
}

rt_inline void dji_motor_list_append(rt_slist_t *l, rt_slist_t *n)
{
    struct rt_slist_node *node;

    node = l;
    while (node->next) node = node->next;

    /* append the node to the tail */
    node->next = n;
    n->next = RT_NULL;
}
rt_inline void list_append(DJIMotor_t *device)
{
    dji_motor_list_init(&(device->list));
    dji_motor_list_append(&dji_motor_list, &(device->list));
}



/**
 * @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
 *        该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
 *
 * C610(m2006)/C620(m3508):0x1ff,0x200;
 * GM6020:0x1ff,0x2ff 0x1fe 0x2fe
 * 反馈(rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
 * can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF [3]:0x1fe
 * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF 
 */



static CanMessage sender_assignment[10] = {
    [0] = {.can_handle = &can1_bus, .txmsg.id =0x1ff, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [1] = {.can_handle = &can1_bus, .txmsg.id =0x200, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [2] = {.can_handle = &can1_bus, .txmsg.id =0x2ff, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [3] = {.can_handle = &can1_bus, .txmsg.id =0x1fe, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [4] = {.can_handle = &can1_bus, .txmsg.id =0x2fe, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},

    [5] = {.can_handle = &can2_bus, .txmsg.id =0x1ff, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [6] = {.can_handle = &can2_bus, .txmsg.id =0x200, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [7] = {.can_handle = &can2_bus, .txmsg.id =0x2ff, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [8] = {.can_handle = &can2_bus, .txmsg.id =0x1fe, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
    [9] = {.can_handle = &can2_bus, .txmsg.id =0x2fe, .txmsg.ide = RT_CAN_STDID ,.txmsg.rtr = RT_CAN_DTR , .txmsg.len =8 ,.txmsg.data = {0},},
};

/**
 * @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
 *        flag的初始化在 MotorSenderGrouping()中进行
 */
static uint8_t sender_enable_flag[10] = {0};

/**
 * @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
 *        并对电机进行分组以便处理多电机控制命令
 */
static void MotorSenderGrouping(DJIMotor_t *motor, CanMessage_Init_s *config)
{
    uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
    uint8_t motor_send_num;
    uint8_t motor_grouping;
    rt_slist_t *node = RT_NULL;
    DJIMotor_t *djidevice = RT_NULL;

    switch (motor->motor_type)
    {
    case M2006:
    case M3508:
        if (motor_id < 4) // 根据ID分组
        {
            motor_send_num = motor_id;
            motor_grouping = config->can_handle == &can1_bus ? 1 : 6;
        }
        else
        {
            motor_send_num = motor_id - 4;
            motor_grouping = config->can_handle == &can1_bus ? 0 : 5;
        }

        // 计算接收id并设置分组发送id
        config->rx_id = 0x200 + motor_id + 1;   // 把ID+1,进行分组设置
        sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
        motor->message_num = motor_send_num;
        motor->sender_group = motor_grouping;


        rt_slist_for_each(node, &dji_motor_list)
        {
            djidevice = rt_slist_entry(node,DJIMotor_t, list);// 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8)
            if (config->rx_id == djidevice->CanMessage_Init.rx_id && config->can_handle == djidevice->CanMessage_Init.can_handle && config->tx_id != djidevice->CanMessage_Init.tx_id)
            {
                LOG_E("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.\n");
                RT_ASSERT(config->rx_id  == djidevice->CanMessage_Init.rx_id && config->can_handle == djidevice->CanMessage_Init.can_handle);
            }
        }
        break;


    case GM6020_CURRENT:
        if (motor_id < 4)
        {
            motor_send_num = motor_id;
            motor_grouping = config->can_handle == &can1_bus ? 3 : 8;
        }
        else
        {
            motor_send_num = motor_id - 4;
            motor_grouping = config->can_handle == &can1_bus ? 2 : 7;
        }

        config->rx_id = 0x204 + motor_id + 1;   // 把ID+1,进行分组设置
        sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
        motor->message_num = motor_send_num;
        motor->sender_group = motor_grouping;
        rt_slist_for_each(node, &dji_motor_list)
        {
            djidevice = rt_slist_entry(node,DJIMotor_t, list);// 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8)
            if (config->rx_id  == djidevice->CanMessage_Init.rx_id && config->can_handle == djidevice->CanMessage_Init.can_handle && config->tx_id != djidevice->CanMessage_Init.tx_id)
            {
                LOG_E("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.\n");
                RT_ASSERT(config->rx_id  == djidevice->CanMessage_Init.rx_id && config->can_handle == djidevice->CanMessage_Init.can_handle); 
            }
        }
        break;

    case GM6020_VOLTAGE:
        if (motor_id < 4)
        {
            motor_send_num = motor_id;
            motor_grouping = config->can_handle == &can1_bus ? 0 : 5;
        }
        else
        {
            motor_send_num = motor_id - 4;
            motor_grouping = config->can_handle == &can1_bus ? 4 : 9;
        }

        config->rx_id = 0x204 + motor_id + 1;   // 把ID+1,进行分组设置
        sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
        motor->message_num = motor_send_num;
        motor->sender_group = motor_grouping;
        rt_slist_for_each(node, &dji_motor_list)
        {
            djidevice = rt_slist_entry(node,DJIMotor_t, list);// 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8)
            if (config->rx_id  == djidevice->CanMessage_Init.rx_id && config->can_handle == djidevice->CanMessage_Init.can_handle && config->tx_id != djidevice->CanMessage_Init.tx_id)
            {
                LOG_E("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.\n");
                RT_ASSERT(config->rx_id  == djidevice->CanMessage_Init.rx_id && config->can_handle == djidevice->CanMessage_Init.can_handle); 
            }
        }
        break;

    }
}

/**
 * @todo  是否可以简化多圈角度的计算？
 * @brief 根据返回的can_instance对反馈报文进行解析
 *
 * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
 */
void DecodeDJIMotor(struct rt_can_msg can_msg)
{
    rt_slist_t *node = RT_NULL;
    DJIMotor_t *djidevice = RT_NULL;
    rt_uint32_t current_time = rt_tick_get(); // 获取当前系统节拍数

    rt_slist_for_each(node, &dji_motor_list)
    {
        djidevice = rt_slist_entry(node,DJIMotor_t, list);
        //LOG_D("djidevice address: %p\n",djidevice);
        // 检查 djidevice 是否为 RT_NULL
        if (djidevice == RT_NULL)
        {
            LOG_E("Invalid djidevice pointer\n");
            continue;
        }
        if (can_msg.id == djidevice->CanMessage_Init.rx_id)
        {
            offline_event_time_update(djidevice->offline_manage_dji.event);
            if (djidevice->motor_settings.Feedback_Object_Type ==MOTOR_ROTOR_FEEDBACK)
            {
                djidevice->measure.last_ecd = djidevice->measure.ecd;
                djidevice->measure.ecd = ((uint16_t)can_msg.data[0]) << 8 | can_msg.data[1];
                djidevice->measure.angle_single_round = ECD_ANGLE_COEF_DJI * (float)djidevice->measure.ecd;
                djidevice->measure.speed_rpm = (1.0f - SPEED_SMOOTH_COEF) * djidevice->measure.speed_rpm + SPEED_SMOOTH_COEF*(float)((int16_t)(can_msg.data[2] << 8 | can_msg.data[3]));
                djidevice->measure.speed_aps = (1.0f - SPEED_SMOOTH_COEF) * djidevice->measure.speed_aps +RPM_2_ANGLE_PER_SEC *SPEED_SMOOTH_COEF *(float)((int16_t)(can_msg.data[2] << 8 | can_msg.data[3]));
                djidevice->measure.real_current = (1.0f - CURRENT_SMOOTH_COEF) * djidevice->measure.real_current +
                                        CURRENT_SMOOTH_COEF * (float)((int16_t)(can_msg.data[4] << 8 | can_msg.data[5]));
                djidevice->measure.temperature = can_msg.data[6];

                // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
                if (djidevice->measure.ecd - djidevice->measure.last_ecd > 4096)
                    djidevice->measure.total_round--;
                else if (djidevice->measure.ecd - djidevice->measure.last_ecd < -4096)
                    djidevice->measure.total_round++;
                djidevice->measure.total_angle = djidevice->measure.total_round * 360 + djidevice->measure.angle_single_round; 
            }
            else
            {
                if (djidevice->motor_type == M2006)
                {
                    djidevice->measure.last_ecd = djidevice->measure.ecd;
                    djidevice->measure.ecd = ((uint16_t)can_msg.data[0]) << 8 | can_msg.data[1];
                    djidevice->measure.angle_single_round = ECD_ANGLE_COEF_DJI * (float)djidevice->measure.ecd;
                    // 计算转子的 RPM
                    int16_t raw_rpm = (int16_t)(can_msg.data[2] << 8 | can_msg.data[3]);
                    float rotor_rpm = (float)raw_rpm;
                    // 计算输出轴的 RPM
                    djidevice->measure.speed_rpm = ((1.0f - SPEED_SMOOTH_COEF) * djidevice->measure.speed_rpm + SPEED_SMOOTH_COEF * rotor_rpm) / 36.0f;
                    // 计算输出轴的角速度 (度每秒)
                    djidevice->measure.speed_aps = ((1.0f - SPEED_SMOOTH_COEF) * djidevice->measure.speed_aps + RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * rotor_rpm) / (36.0f * 10.0f);//角速度缩放
                    // 计算实际电流
                    djidevice->measure.real_current = (1.0f - CURRENT_SMOOTH_COEF) * djidevice->measure.real_current +
                                                    CURRENT_SMOOTH_COEF * (float)((int16_t)(can_msg.data[4] << 8 | can_msg.data[5]));
                    // 计算温度
                    djidevice->measure.temperature = can_msg.data[6];
                    // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
                    if (djidevice->measure.ecd - djidevice->measure.last_ecd > 4096)
                        djidevice->measure.total_round--;
                    else if (djidevice->measure.ecd - djidevice->measure.last_ecd < -4096)
                        djidevice->measure.total_round++;

                    // 计算输出轴的总圈数
                    djidevice->measure.total_output_round = djidevice->measure.total_round / 36.0f;

                    // 计算总角度
                    djidevice->measure.total_angle = djidevice->measure.total_output_round * 360.0f + (djidevice->measure.angle_single_round) / 36.0f;
                }
                if (djidevice->motor_type  == M3508)
                {
                    djidevice->measure.last_ecd = djidevice->measure.ecd;
                    djidevice->measure.ecd = ((uint16_t)can_msg.data[0]) << 8 | can_msg.data[1];
                    djidevice->measure.angle_single_round = ECD_ANGLE_COEF_DJI * (float)djidevice->measure.ecd;
                    // 计算转子的 RPM
                    int16_t raw_rpm = (int16_t)(can_msg.data[2] << 8 | can_msg.data[3]);
                    float rotor_rpm = (float)raw_rpm;
                    // 计算输出轴的 RPM
                    djidevice->measure.speed_rpm = ((1.0f - SPEED_SMOOTH_COEF) * djidevice->measure.speed_rpm + SPEED_SMOOTH_COEF * rotor_rpm) / 19.0f;
                    // 计算输出轴的角速度 (度每秒)
                    djidevice->measure.speed_aps = ((1.0f - SPEED_SMOOTH_COEF) * djidevice->measure.speed_aps + RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * rotor_rpm) / 19.0f;
                    // 计算实际电流
                    djidevice->measure.real_current = (1.0f - CURRENT_SMOOTH_COEF) * djidevice->measure.real_current +
                                                    CURRENT_SMOOTH_COEF * (float)((int16_t)(can_msg.data[4] << 8 | can_msg.data[5]));
                    // 计算温度
                    djidevice->measure.temperature = can_msg.data[6];
                    // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
                    if (djidevice->measure.ecd - djidevice->measure.last_ecd > 4096)
                        djidevice->measure.total_round--;
                    else if (djidevice->measure.ecd - djidevice->measure.last_ecd < -4096)
                        djidevice->measure.total_round++;

                    // 计算输出轴的总圈数
                    djidevice->measure.total_output_round = djidevice->measure.total_round / 19.0f;

                    // 计算总角度
                    djidevice->measure.total_angle = djidevice->measure.total_output_round * 360.0f + (djidevice->measure.angle_single_round) / 19.0f;
                }
            }
        }
    }
}

// 电机初始化,返回一个电机实例
DJIMotor_t *DJIMotorInit(Motor_Init_Config_s *config)
{
    DJIMotor_t *DJIMotor = (DJIMotor_t *)rt_malloc(sizeof(DJIMotor_t));
    if (DJIMotor == RT_NULL) {
        LOG_E("Failed to allocate memory for DJIMotor\n");
        return RT_NULL;
    }
    rt_memset(DJIMotor, 0, sizeof(DJIMotor_t));
    list_append(DJIMotor);

    // motor basic setting 电机基本设置
    DJIMotor->motor_type = config->motor_type;                         // 6020 or 2006 or 3508
    DJIMotor->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
    DJIMotor->CanMessage_Init =config->can_init_config;
    DJIMotor->motor_settings.control_algorithm = config->controller_setting_init_config.control_algorithm;

    switch (config->controller_setting_init_config.control_algorithm) {
        case CONTROL_PID:
            // motor controller init 电机控制器初始化
            PIDInit(&DJIMotor->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
            PIDInit(&DJIMotor->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
            PIDInit(&DJIMotor->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
            DJIMotor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
            DJIMotor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
            DJIMotor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
            DJIMotor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
            break;
        case CONTROL_LQR:
            LQRInit(&DJIMotor->motor_controller.lqr, &config->controller_param_init_config.lqr_config);
            DJIMotor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
            DJIMotor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
            DJIMotor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
            DJIMotor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
            break;
        case CONTROL_OTHER:
            // 未来添加其他控制算法的初始化
            break;
    }

    // 电机分组,因为至多4个电机可以共用一帧CAN控制报文
    MotorSenderGrouping(DJIMotor, &DJIMotor->CanMessage_Init);
    //DJIMotorEnable(DJIMotor);

    //掉线检测
    DJIMotor->offline_manage_dji.event =config->offline_manage_motor.event;
    offline_event_init(config->offline_manage_motor);

    // 打印内存地址
    // LOG_D("Motor address: %p\n", DJIMotor);
    // LOG_D("CanMessage_Init address: %p\n", &DJIMotor->CanMessage_Init);
    // LOG_D("Measure address: %p\n", &DJIMotor->measure);

    return DJIMotor;
}

/* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
void DJIMotorChangeFeed(DJIMotor_t *motor, Closeloop_Type_e loop, Feedback_Source_e type)
{
    if (loop == ANGLE_LOOP)
        motor->motor_settings.angle_feedback_source = type;
    else if (loop == SPEED_LOOP)
        motor->motor_settings.speed_feedback_source = type;
    else
        LOG_E("[dji_motor] loop type error, check memory access and func param\n"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
}

void DJIMotorStop(DJIMotor_t *motor)
{
    motor->stop_flag = MOTOR_STOP;
}

void DJIMotorEnable(DJIMotor_t *motor)
{
    motor->stop_flag = MOTOR_ENALBED;
}

/* 修改电机的实际闭环对象 */
void DJIMotorOuterLoop(DJIMotor_t *motor, Closeloop_Type_e outer_loop)
{
    motor->motor_settings.outer_loop_type = outer_loop;
}

// 设置参考值
void DJIMotorSetRef(DJIMotor_t *motor, float ref)
{
    switch (motor->motor_settings.control_algorithm) 
    {
        case CONTROL_PID:
            motor->motor_controller.pid_ref = ref;
            break;
        case CONTROL_LQR:
            motor->motor_controller.lqr_ref = ref;
            break;
        case CONTROL_OTHER:
            break;
    }
}
/*

*/
rt_inline int16_t currentToInteger(float I_min,float I_max,int16_t V_min,int16_t V_max,float current) {
    int16_t V = (int)((current - I_min) / (I_max - I_min) * (V_max - V_min) + V_min);
    if (V >V_max)
    {
        V=V_max;
    }
    if (V<V_min)
    {
        V=V_min;
    }
    return V;
}


void DJIMotorControl()
{
    uint8_t group, num; // 电机组号和组内编号
    int16_t set;        // 电机控制CAN发送设定值
    DJIMotor_t *motor = RT_NULL;
    float pid_measure, pid_ref;             // 电机PID测量值和设定值
    float state0, state1;

    // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
    rt_slist_t *node = RT_NULL;

    // 调用 PowerControlDji 进行功率控制计算
    rt_slist_for_each(node, &dji_motor_list)
    {
        motor = rt_slist_entry(node, DJIMotor_t, list);
        if (motor->motor_settings.PowerControlState == PowerControlState_ON)
        {
            PowerControlDji(motor);  // 在powercontrol中处理
        }
    }

    // 调用 PowerControlDjiFinalize 进行功率分配
    PowerControlDjiFinalize(&dji_motor_list);

    rt_slist_for_each(node, &dji_motor_list)
    {
        motor = rt_slist_entry(node, DJIMotor_t, list);
        if (motor->motor_settings.PowerControlState == PowerControlState_ON)
        {
            if (motor->offline_manage_dji.online_state == STATE_OFFLINE || motor->stop_flag == MOTOR_STOP) // 如果电机处于离线状态,发送0 若该电机处于停止状态,直接将buff置零
            {
                set = 0;
            }
            else
            {
                // 从 powercontrol 获取每个电机的功率控制输出
                set = GetPowerControlOutput(motor->message_num);
            }
        }
        else
        {
            if (motor->offline_manage_dji.online_state == STATE_OFFLINE || motor->stop_flag == MOTOR_STOP) // 如果电机处于离线状态,发送0 若该电机处于停止状态,直接将buff置零
            {
                set = 0;
            }
            else 
            {
                switch (motor->motor_settings.control_algorithm) {
                    case CONTROL_PID:
                    {
                        pid_ref = motor->motor_controller.pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
                        if (motor->motor_settings.motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
                            pid_ref *= -1;                         // 设置反转

                        // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
                        if ((motor->motor_settings.close_loop_type & ANGLE_LOOP) && motor->motor_settings.outer_loop_type == ANGLE_LOOP)
                        {
                            if (motor->motor_settings.angle_feedback_source == OTHER_FEED)
                                pid_measure = *motor->motor_controller.other_angle_feedback_ptr;
                            else
                                pid_measure = motor->measure.total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
                            // 更新pid_ref进入下一个环
                            pid_ref = PIDCalculate(&motor->motor_controller.angle_PID, pid_measure, pid_ref);
                        }

                        // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
                        if ((motor->motor_settings.close_loop_type & SPEED_LOOP) && (motor->motor_settings.outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
                        {
                            if (motor->motor_settings.feedforward_flag & SPEED_FEEDFORWARD)
                                pid_ref += *motor->motor_controller.speed_feedforward_ptr;

                            if (motor->motor_settings.speed_feedback_source == OTHER_FEED)
                                pid_measure = *motor->motor_controller.other_speed_feedback_ptr;
                            else // MOTOR_FEED
                                pid_measure = motor->measure.speed_rpm;
                            // 更新pid_ref进入下一个环
                            pid_ref = PIDCalculate(&motor->motor_controller.speed_PID, pid_measure, pid_ref);
                        }

                        // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
                        if (motor->motor_settings.feedforward_flag & CURRENT_FEEDFORWARD)
                            pid_ref += *motor->motor_controller.current_feedforward_ptr;
                        if (motor->motor_settings.close_loop_type & CURRENT_LOOP)
                        {
                            pid_ref = PIDCalculate(&motor->motor_controller.current_PID, motor->measure.real_current, pid_ref);
                        }

                        if (motor->motor_settings.feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
                            pid_ref *= -1;

                        // 获取最终输出
                        set = (int16_t)pid_ref;
                        break;
                    }
                    case CONTROL_LQR:
                    {
                        if(motor->motor_settings.feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
                        {
                            motor->motor_controller.lqr.feedbackreverseflag = 1;
                        }

                        // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
                        if ((motor->motor_settings.close_loop_type & ANGLE_LOOP) && motor->motor_settings.outer_loop_type == ANGLE_LOOP)
                        {   
                            if (motor->motor_settings.angle_feedback_source == OTHER_FEED) {state0 = *motor->motor_controller.other_angle_feedback_ptr;}
                            else {state0 = motor->measure.total_angle;} // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
                        }

                        if ((motor->motor_settings.close_loop_type & SPEED_LOOP) && (motor->motor_settings.outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
                        {   
                            if (motor->motor_settings.speed_feedback_source == OTHER_FEED)  {state1 = *motor->motor_controller.other_speed_feedback_ptr;}
                            else  {state1 = motor->measure.speed_aps; if (motor->motor_type ==GM6020_CURRENT){state1 = state1/50;}}
                        }

                        float torque = LQRCalculate(&motor->motor_controller.lqr, state0, state1, motor->motor_controller.lqr_ref);
                        switch (motor->motor_type) 
                        {                                          
                            case GM6020_CURRENT:
                                {
                                    set = currentToInteger(-3.0f, 3.0f, -16384, 16384, (torque / 0.741f)); // 力矩常数（Nm/A）
                                    break;
                                }
                            case M3508:
                                {
                                    set = currentToInteger(-20.0f, 20.0f, -16384, 16384, (torque / (0.3f))); // 力矩常数（Nm/A）
                                    break;                                
                                }
                            case M2006:
                                {
                                    set = currentToInteger(-10.0f, 10.0f, -10000, 10000, (torque / (0.18f))); // 力矩常数（Nm/A）                                
                                    break;                                   
                                }
                            default :
                                set = 0;
                                break;
                        }
                    }

                    case CONTROL_OTHER:
                        // 待加入
                        break;
                    default:
                        break;
                }
            }
        }

        // 分组填入发送数据
        group                                            = motor->sender_group;
        num                                              = motor->message_num;
        sender_assignment[group].txmsg.data[2 * num]     = (uint8_t)(set >> 8);     // 低八位
        sender_assignment[group].txmsg.data[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
    }

    // 遍历flag,检查是否要发送这一帧报文
    for (size_t i = 0; i < 10; ++i)
    {
        if (sender_enable_flag[i])
        {
            rt_device_write(*sender_assignment[i].can_handle, 0, &sender_assignment[i].txmsg, sizeof(sender_assignment[i].txmsg));
        }
    }
}